//
// Created by Pulsar on 2020/5/16.
//
#include <libserv/libserv.h>
#include <iostream>
#include <string>
#include <cstring>
#include <ctime>
#include "JY901.h"

using namespace libserv;
using namespace std;

int test_function(void *arg, std::vector<char> message);

bool check_function(std::vector<char> package);

#include <zconf.h>

bool check_function(std::vector<char> package) {
    return true;
}

//int test_function(void *arg, std::vector<char> message){
//    if (message.size() < 11)return 0;
//    char *temp_char = new char[message.size()];
//    std::cout<<"Recive<-"<<clock();
//    for (int i = 0; i < message.size() - 1; i++) {
//        temp_char[i] = message[i];
//        std::cout<<"\\x"<<std::hex<<(int)temp_char[i];
//    }
//    std::cout<<std::endl;
//}
int test_function(void *arg, std::vector<char> message) {
    if (message.size() < 11)return 0;
    char *temp_char = new char[message.size()];
    for (int i = 0; i < message.size() - 1; i++) {
        temp_char[i] = message[i];
    }
    struct STime stcTime;
    struct SAcc stcAcc;
    struct SGyro stcGyro;
    struct SAngle stcAngle;
    struct SMag stcMag;
    struct SDStatus stcDStatus;
    struct SPress stcPress;
    struct SLonLat stcLonLat;
    struct SGPSV stcGPSV;
    system("clear");
    switch (message[1]) {
        case 0x50:
            memcpy(&stcTime, &temp_char[2], 8);
//            printf("Time:20%d-%d-%d %d:%d:%.3f\r\n", (short)stcTime.ucYear, (short)stcTime.ucMonth,
//                   (short)stcTime.ucDay, (short)stcTime.ucHour, (short)stcTime.ucMinute, (float)stcTime.ucSecond + (float)stcTime.usMiliSecond / 1000);
            break;
        case 0x51:
            memcpy(&stcAcc, &temp_char[2], 8);

            std::cout << "Acc:"
                      << " " << (float) stcAcc.a[0] / 32768 * 16
                      << " " << (float) stcAcc.a[1] / 32768 * 16
                      << " " << (float) stcAcc.a[2] / 32768 * 16 << "\n";

//            printf("Acc:%.3f %.3f %.3f\r\n", (float) stcAcc.a[0] / 32768 * 16, (float) stcAcc.a[1] / 32768 * 16,
//                   (float) stcAcc.a[2] / 32768 * 16);
            break;
        case 0x52:
            memcpy(&stcGyro, &temp_char[2], 8);
            std::cout << "Gyro:"
                      << " " << (float) stcGyro.w[0] / 32768 * 2000
                      << " " << (float) stcGyro.w[1] / 32768 * 2000
                      << " " << (float) stcGyro.w[2] / 32768 * 2000 << "\n";
//            printf("Gyro:%.3f %.3f %.3f\r\n", (float)stcGyro.w[0] / 32768 * 2000, (float)stcGyro.w[1] / 32768 * 2000, (float)stcGyro.w[2] / 32768 * 2000);
            break;
        case 0x53:
            memcpy(&stcAngle, &temp_char[2], 8);
            std::cout << "Angle:"
                      << " " << (float) stcAngle.Angle[0] / 32768 * 180
                      << " " << (float) stcAngle.Angle[1] / 32768 * 180
                      << " " << (float) stcAngle.Angle[2] / 32768 * 180 << "\n";
//            printf("Angle:%.3f %.3f %.3f\r\n", (float)stcAngle.Angle[0] / 32768 * 180, (float)stcAngle.Angle[1] / 32768 * 180, (float)stcAngle.Angle[2] / 32768 * 180);
            break;
        case 0x54:
            memcpy(&stcMag, &temp_char[2], 8);
            std::cout << "Mag:"
                      << " " << stcMag.h[0]
                      << " " << stcMag.h[1]
                      << " " << stcMag.h[2] << "\n";
//            printf("Mag:%d %d %d\r\n", stcMag.h[0], stcMag.h[1], stcMag.h[2]);
            break;
        case 0x55:
            memcpy(&stcDStatus, &temp_char[2], 8);
            std::cout << "DStatus:"
                      << " " << stcDStatus.sDStatus[0]
                      << " " << stcDStatus.sDStatus[1]
                      << " " << stcDStatus.sDStatus[2]
                      << " " << stcDStatus.sDStatus[3] << "\n";
//            printf("DStatus:%d %d %d %d\r\n", stcDStatus.sDStatus[0], stcDStatus.sDStatus[1], stcDStatus.sDStatus[2], stcDStatus.sDStatus[3]);
            break;
        case 0x56:
            memcpy(&stcPress, &temp_char[2], 8);
            std::cout << "Pressure:"
                      << " " << stcPress.lPressure
                      << " " << (float) stcPress.lAltitude / 100 << "\n";
//            printf("Pressure:%lx Height%.2f\r\n", stcPress.lPressure, (float)stcPress.lAltitude / 100);
            break;
        case 0x57:
            memcpy(&stcLonLat, &temp_char[2], 8);
//            printf("Longitude:%ldDeg%.5fm Lattitude:%ldDeg%.5fm\r\n", stcLonLat.lLon / 10000000, (double)(stcLonLat.lLon % 10000000) / 1e5, stcLonLat.lLat / 10000000, (double)(stcLonLat.lLat % 10000000) / 1e5);
            break;
        case 0x58:
            memcpy(&stcGPSV, &temp_char[2], 8);
//            printf("GPSHeight:%.1fm GPSYaw:%.1fDeg GPSV:%.3fkm/h\r\n\r\n", (float)stcGPSV.sGPSHeight / 10, (float)stcGPSV.sGPSYaw / 10, (float)stcGPSV.lGPSVelocity / 1000);
            break;
    }
    delete temp_char;
}

int main(int argc, char **argv) {
    Serial serial("/dev/ttyUSB1", 9600);
    serial.bind_call(test_function);
    serial.bind_package_check(check_function);
    serial.run();
    int counter = 12;
    while (true) {
//        sleep(300);
//        char speed = (char)counter;
//        //char buffer[5] = { 0xff,0xaa,0x03,speed,0x00 };
//        char buffer[16] = "Udadsawqcascxz";
//        serial.ansy_send(buffer, 16);
    }
}